/**
 * sketch_fontanabot_ik_system
 *
 * James Stone      http://www.manofstone.com
 *
 * Articulates a Lynx6 Robotic Arm, Using a pololu micro servo board
 * via an Arduino, using arduino sketch:
 *
 * sketch_servo_and_serial_com.pde
 * 
 * Based on Reach 1. 
 * Based on code from Keith Peters (www.bit-101.com)
 * 
 * The arm follows the position of the mouse by
 * calculating the angles with atan2(). 
 */

import processing.serial.*;

// String portname = "/dev/tty.usbserial-A5002rKW"; // james arduino
//  String portname = "/dev/tty.usbserial-A1001NQr"; // thomas' arduino, w/ feet
 String portname = "/dev/tty.SerialPort-1"; // for testing
Serial port;  // Create object from Serial class





float zCanvasLoc = 155;  // z loc of piece to cut
float xCanvasWidth = 100;  // width of canvas


void setup() {
  size(200, 200);
  smooth(); 
  strokeWeight(1.0);
  stroke(0, 100);
  PFont fontA = loadFont("Ziggurat-HTF-Black-32.vlw");
  textFont(fontA, 12);
  
  // init serial
  // port = new Serial(this, portname, 9600);
}

// servo 0, rotation of base
String angle2Servo0(float a) { // a = angle in radians    
   int center = 2830; // centered rotation
   int c90 = 4520; // 90deg to the right, clockwise
   int cc90 = 1140; // 90deg to the left, counter clockwise, calculated
   int value = center;  // if below is not met, a = 0 exactly
   if (a > 0) { // clockwise + angles
      value = (int)((c90 - center) / 90 * (degrees(a)) + center);
   } else if (a < 0) { // cc - angles
     value = (int)(center - (center - cc90) / 90 * abs((degrees(a))));
   }
  return "s0" + nf(value, 4); 
}

// servo 1, elevation of 1st arm segment from base, shoulder
String angle2Servo1(float a) { // a = angle in radians    
   int center = 1280; // parallel with the ground plane
   int c90 = 3030; // perpendicular to the ground plane
   int value = center;  // if below is not met, a = 0 exactly
      value = (int)((c90 - center) / 90 * (degrees(a)) + center);
  return "s1" + nf(value, 4); 
}

// servo 2, elevation of 2nd arm segment from base, elbow
String angle2Servo2(float a) { // a = angle in radians    
   int center = 3210; // centered rotation
   int c90 = 4860; // 90deg to the right, clockwise
   int cc90 = 1560; // 90deg to the left, counter clockwise, calculated
   int value = center;  // if below is not met, a = 0 exactly
   if (a > 0) { // clockwise + angles
      value = (int)((c90 - center) / 90 * (degrees(a)) + center);
   } else if (a < 0) { // cc - angles
     value = (int)(center - (center - cc90) / 90 * abs((degrees(a))));
   }
  return "s2" + nf(value, 4); 
}

void moveArm(float x, float y, float z) {
  
  float seg1length = 50;
  float seg2length = 50;
  float resol = 1;

  float dx =  z;  // use coord system
  float dy =  y;
  float dz =  z;


  int x2 = 100;
  int y2 = 100;
 // float dx = z;
//  float dy = y;
  float angle1 = atan2(dy, dx);  
  
  float tx = mouseX - cos(angle1) * seg1length;
  float ty = mouseY - sin(angle1) * seg1length;
  dx = tx - x2;
  dy = ty - y2;
  float angle2 = atan2(dy, dx);  
  x = x2 + cos(angle2) * seg1length;
  y = y2 + sin(angle2) * seg1length;
  
  segment(x, y, seg1length, angle1); 
  segment(x2, y2, seg1length, angle2); 
  
  
/*  float angle1 = atan2(y, z);   // find angle of first seg
  float tz = z - cos(angle1) * seg1length;  // find projection of segment, start of segment 2
  float ty = y - sin(angle1) * seg1length;
  dz = tz - 0;
  dy = ty - 0;

float angle2 = atan2(dy, dz);  
  float lz = 0 + cos(angle2) * seg2length;
  float ly = 0 + sin(angle2) * seg2length;  
  
     int offsetx = width/2;
     int offsety = height/2;
     segment(offsetx + lz, offsetx + ly, seg2length, angle1);
     segment(offsetx, offsety, seg1length, angle2);
     */
  /*   rect(offsetx - 2, offsety - 2, 4, 4);
     fill(255, 0, 0);
     rect(offsetx + lz - 2, offsety + ly - 2, 4, 4);
          fill(0, 255, 0); */
 //    rect(offsetx + z - 2, offsety + y - 2, 4, 4);
          fill(0, 0, 255);
//     rect(offsetx + lz - 2, offsety + ly - 2, 4, 4);  //lz & ly is the start point of the second segment
  /*

  float angle3 = -atan2(x, z); //+ radians(90);  
  float xCan = cos(angle2) * z;
  float yCan = sin(angle2) * z;
  float zLen = 0; // length  
  float rotOffsetLength = sqrt(pow(z, 2) + pow(abs(xCan), 2)) - z;// amount to adjust other algorhythms
  
  
  dx =  x;  // reset system
  dy =  y;
  dz =  z + rotOffsetLength; // increase distance to account for base rotation
  
  /*
  
  /*
  float angle1corr = atan2(dy, dz);   // calculates in radians pi to - pi
  tz = z - cos(angle1corr) * seg1length;
  ty = y - sin(angle1corr) * seg1length;
  dz = tz;
  dy = ty;
  float angle2corr = atan2(dy, dz);  
  lz = cos(angle2corr) * seg2length;
  ly = sin(angle2corr) * seg2length;  
   */

     
     



  text("x=" + x + " y=" + y + " z=" + z, 50, 25);
  //  text("angle2Servo0(angle3)=" + angle2Servo0(angle2), 50, 50);
  /*  text("angle2Servo1(angle2corr)=" + angle2Servo1(angle2corr), 50, 75);
    text("angle2Servo2(angle1corr)=" + angle2Servo2(angle1corr), 50, 100); */
   text("angle1=" + degrees(angle1), 50, 125);
    text("angle2=" + degrees(angle2), 50, 150);
 /*   text("angle3=" + degrees(angle3), 50, 175); */
 /*   text("angle1corr=" + degrees(angle1corr), 50, 200);
    text("angle2corr=" + degrees(angle2corr), 50, 225); */

   // angle1 = elbow angle, angle 2 = shoulder angle, angle3 = rotation of base
   // move servos after all calcs
/*   // move servo 1, 1st arm seg
   port.write(angle2Servo1(angle2corr));
   // move servo 2, 2nd arm seg
   port.write(angle2Servo2(angle1corr));
   // move servo 0
   port.write(angle2Servo0(angle3));
   */
}
  int x = 0;
  int y = -100;
  int z = 10;
void draw() {

  background(100);
  fill(0);

  //moveArm(x, y, z);
  moveArm(x, mouseY, mouseX);
  y++;
  if (y == 0) {
   y = -100; 
  }
 /* x++;
  if (x == 100) {
   x = 0; 
  } */
  
 delay(100);

}

void segment(float x, float y, float segLength,  float a) {
  pushMatrix();
  translate(x, y); // redefine the origin
  rotate(a);       // rotate in radians
  line(0, 0, segLength, 0);  // draw line the seglength
  popMatrix();
}
